#include "bsp_servo.h"

extern TIM_HandleTypeDef htim2;

Servo_t Servo_yaw;
Servo_t Servo_pitch;
Servo_t Servo_roll;
Servo_t Servo_catch;

#define servo_angle_limit(input, output, min,max)        \
    {                                                    \
        if ((input) > (min) || (input) < (max))          \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else if((input) < (min))                         \
        {                                                \
            (output) = (min);                            \
        }                                                \
        else if((input) > (max))                         \
        {                                                \
            (output) = (max);                            \
        }                                                \
    }

void Servo_Init(void)
{
    

    Servo_yaw.tim = SERVO_YAW_TIM;
    Servo_yaw.tim_channel = SERVO_YAW_CHANNEL;
    HAL_TIM_PWM_Start(&Servo_yaw.tim, Servo_yaw.tim_channel);

    Servo_pitch.tim = SERVO_PITCH_TIM;
    Servo_pitch.tim_channel = SERVO_PITCH_CHANNEL;
    HAL_TIM_PWM_Start(&Servo_pitch.tim,Servo_pitch.tim_channel);

    Servo_roll.tim = SERVO_ROLL_TIM;
    Servo_roll.tim_channel = SERVO_ROLL_CHANNEL;
    HAL_TIM_PWM_Start(&Servo_roll.tim,Servo_roll.tim_channel);

    Servo_catch.tim = SERVO_CATCH_TIM;
    Servo_catch.tim_channel = SERVO_CATCH_CHANNEL;
    HAL_TIM_PWM_Start(&Servo_catch.tim,Servo_catch.tim_channel);
}

void Servo_Control(void)
{
    Servo_angle_ccr_calc(&Servo_yaw);
    __HAL_TIM_SET_COMPARE(&Servo_yaw.tim,Servo_yaw.tim_channel,Servo_yaw.ccr);

        Servo_angle_ccr_calc(&Servo_pitch);
    __HAL_TIM_SET_COMPARE(&Servo_pitch.tim,Servo_pitch.tim_channel,Servo_pitch.ccr);
    
       Servo_angle_ccr_calc(&Servo_roll);
   __HAL_TIM_SET_COMPARE(&Servo_roll.tim,Servo_roll.tim_channel,Servo_roll.ccr);
    
        Servo_angle_ccr_calc(&Servo_catch);
    __HAL_TIM_SET_COMPARE(&Servo_catch.tim,Servo_catch.tim_channel,Servo_catch.ccr);


}

static void Servo_angle_ccr_calc(Servo_t* servo)
{
    servo_angle_limit(servo ->angle_set,servo ->angle_output,0.0f,180.0f);
    servo ->ccr = servo ->angle_output * 200/180 +50;
}

Servo_t* Return_Servo_yaw_point(void)
{
    return &Servo_yaw;
}

Servo_t* Return_Servo_pitch_point(void)
{
    return &Servo_pitch;
}

Servo_t* Return_Servo_roll_point(void)
{
    return &Servo_roll;
}

Servo_t* Return_Servo_catch_point(void)
{
    return &Servo_catch;
}